#ifndef STEERING_MANAGER_HPP
#define STEERING_MANAGER_HPP

////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include <vector> 

#include <SFML/System/Vector2.hpp>

#include <SmegTools/Config.h>

#include <SmegCore/Math/BoundingVolumes/BoundingBox.hpp>
#include <SmegCore/Math/BoundingVolumes/Line.hpp>

#include "../PointPath.hpp"

#include <SmegTools/Debug/DebugNew.hpp>

#ifdef _DEBUG
namespace sf {
    class RenderWindow;
}
#endif

namespace smeg {
    namespace core {
        class BaseObject;
        class MovableEntity;
    }
    namespace ai {
        
////////////////////////////////////////////////////////////
/// Manage the steering functions called by the entity
////////////////////////////////////////////////////////////
class SteeringManager
{
public:
    enum eComputeMethod {
        eWeighedSums        = 0,
        ePrioritized        = 1,
        eDithered           = 2,
        eMAX_COMPUTE_METHOD
    };
    enum eBehaviors {
        eSeek               = 1 << 0,
        eFlee               = 1 << 1,
        ePursue             = 1 << 2,
        eOffsetPursue       = 1 << 3,
        eEvade              = 1 << 4,
        eArrive             = 1 << 5,
        eWander             = 1 << 6,
        eInterpose          = 1 << 7,
        ePathFollowing      = 1 << 8,
        eSeparation         = 1 << 9,
        eAlignment          = 1 << 10,
        eCohesion           = 1 << 11,
        eWallAvoidance      = 1 << 12,
        eObstacleAvoidance  = 1 << 13,
        //eHide               = 1 << 14
    };
    enum eDeceleration {
        eFast               = 1,
        eNormal             = 2,
        eSlow               = 3,
        eMAX_DECELERATION
    };

    ////////////////////////////////////////////////////////////
    /// Default Constructor
    ///
    /// \param _pEntity : Entity that we manage
    ///
    ////////////////////////////////////////////////////////////
                            SteeringManager( eComputeMethod _eMethod = ePrioritized, core::MovableEntity* _pEntity = NULL );
    ////////////////////////////////////////////////////////////
    /// Copy Constructor
    ///
    /// \param _Copy :      Manager to copy
    ///
    ////////////////////////////////////////////////////////////
                            SteeringManager( const SteeringManager& _Copy );
    ////////////////////////////////////////////////////////////
    /// Destructor
    ///
    ////////////////////////////////////////////////////////////
                            ~SteeringManager();
                            
    ////////////////////////////////////////////////////////////
    /// Update the steering
    ///
    ////////////////////////////////////////////////////////////
    const sf::Vector2f&     Compute();

    /*********************** Behaviors ************************/
    void                    SeekOn( core::MovableEntity* _pTarget, float _fWeight = -1.0f );
    void                    FleeOn( core::MovableEntity* _pTarget, float _fWeight = -1.0f );
    void                    PursueOn( core::MovableEntity* _pTarget, float _fWeight = -1.0f );
    void                    OffsetPursueOn( core::MovableEntity* _pTarget, const sf::Vector2f& _Offset, float _fWeight = -1.0f );
    void                    EvadeOn( core::MovableEntity* _pTarget, float _fWeight = -1.0f );
    void                    ArriveOn( core::MovableEntity* _pTarget, eDeceleration _eDecel = eNormal, float _fArriveThreshold = -1.0f, float _fWeight = -1.0f );
    void                    WanderOn( float _fWanderRadius = 0.8f, float _fWanderDistance = 2.0f, float _fWanderJitter = 50.0f, float _fWeight = -1.0f );
    void                    InterposeOn( core::MovableEntity* _pPrimaryTarget, core::MovableEntity* _pSecondaryTarget, float _fWeight = -1.0f );
    void                    PathFollowingOn( const PointPath& _PointPath, float _fDistanceToWaypoint, float _fWeight = -1.0f );
    void                    SeparationOn( float _fViewRadius = -1.0f, float _fWeight = -1.0f );
    void                    AlignmentOn( float _fViewRadius = -1.0f, float _fWeight = -1.0f );
    void                    CohesionOn( float _fViewRadius = -1.0f, float _fWeight = -1.0f );
    void                    WallAvoidanceOn( float _fFeelerLength = -1.0f, float _fViewRadius = -1.0f, float _fWeight = -1.0f );
    void                    ObstacleAvoidanceOn( float _fBrakingWeight = 0.2f, float _fViewRadius = -1.0f, float _fWeight = -1.0f );
    //void                    HideOn( float _fWeight = -1.0f );

    /*********************** Accessors ************************/
    void                    SetComputeMethod( eComputeMethod _eMethod );

    bool                    IsOn( eBehaviors _eBehavior ) const;
    void                    On( eBehaviors _eBehavior );
    void                    AllOff();
    void                    Off( eBehaviors _eBehavior );

    void                    SetEntity( core::MovableEntity* _pEntity );
    core::MovableEntity*    GetEntity() const;

    const sf::Vector2f&     GetSteeringResult() const;

private:
    // Weighed Sums
    // TODO READ FROM FILE
    float                   m_fSeekWeight;
    float                   m_fFleeWeight;
    float                   m_fPursueWeight;
    float                   m_fOffsetPursueWeight;
    float                   m_fEvadeWeight;
    float                   m_fArriveWeight;
    float                   m_fWanderWeight;
    float                   m_fInterposeWeight;
    float                   m_fPathFollowingWeight;
    float                   m_fSeparationWeight;
    float                   m_fAlignmentWeight;
    float                   m_fCohesionWeight;
    float                   m_fWallAvoidanceWeight;
    float                   m_fObstacleAvoidanceWeight;
    float                   m_fHideWeight;

    // Dithered probabilities
    // TODO READ FROM FILE
    float                   m_fSeekProbability;
    float                   m_fFleeProbability;
    float                   m_fPursueProbability;
    float                   m_fOffsetPursueProbability;
    float                   m_fEvadeProbability;
    float                   m_fArriveProbability;
    float                   m_fWanderProbability;
    float                   m_fInterposeProbability;
    float                   m_fPathFollowingProbability;
    float                   m_fSeparationProbability;
    float                   m_fAlignmentProbability;
    float                   m_fCohesionProbability;
    float                   m_fWallAvoidanceProbability;
    float                   m_fObstacleAvoidanceProbability;
    float                   m_fHideProbability;

    /************************ Compute *************************/
    void                    SetViewRadius( float _fViewRadius );
    bool                    AccumulateForces( sf::Vector2f& _CurrentForce, sf::Vector2f& _ForceToAdd );

    void                    CalculateWeighedSum();
    void                    CalculatePrioritized();
    void                    CalculateDithered();

    /*********************** Behaviors ************************/
    sf::Vector2f            Seek( const sf::Vector2f& _Pos ) const;
    sf::Vector2f            Flee( const sf::Vector2f& _Pos ) const;
    sf::Vector2f            Pursue() const;
    sf::Vector2f            OffsetPursue() const;
    sf::Vector2f            Evade() const;
    sf::Vector2f            Arrive( const sf::Vector2f& _Pos, eDeceleration _eDecel = eNormal, float _fArriveThreshold = -1.0f ) const;
    sf::Vector2f            Wander();
    sf::Vector2f            Interpose() const;
    sf::Vector2f            FollowPath();
    sf::Vector2f            Separation( const std::vector< core::BaseObject* > _Neighbours ) const;
    sf::Vector2f            Alignment( const std::vector< core::BaseObject* > _Neighbours ) const;
    sf::Vector2f            Cohesion( const std::vector< core::BaseObject* > _Neighbours ) const;
    sf::Vector2f            WallAvoidance( const std::vector< core::BaseObject* > _WallObstacles );
    sf::Vector2f            ObstacleAvoidance( const std::vector< core::BaseObject* > _Obstacles ) const;
    // sf::Vector2f            Hide() const;
    
    void                    CreateFeelers();
    void                    CreateWalls();

    /******************* Protected members ********************/
    eComputeMethod          m_eComputeMethod;

    int                     m_iActivatedBehaviors;
    core::MovableEntity*    m_pEntity;
    
    sf::Vector2f            m_SteeringResult;
    
    core::MovableEntity*    m_pSeekPursueTarget;
    sf::Vector2f            m_OffsetPursue;
    core::MovableEntity*    m_pFleeEvadeTarget;
    
    core::MovableEntity*    m_pInterposePrimaryTarget;
    core::MovableEntity*    m_pInterposeSecondaryTarget;

    eDeceleration           m_eArrivalDeceleration;
    float                   m_fArriveThreshold;

    sf::Vector2f            m_WanderTarget;
    float                   m_fWanderRadius;
    float                   m_fWanderDistance;
    float                   m_fWanderJitter;

    PointPath               m_PathToFollow;
    float                   m_fDistanceToWaypointSq;
    
    math::BoundingBox              m_ViewRadiusBox;
    std::vector< core::BaseObject* > m_Neighbours;
    
    std::vector< core::BaseObject* > m_WallObstacles;
    math::Line              m_Feelers[3];
    float                   m_fWallDetectionFeelerLength;
    
    std::vector< core::BaseObject* > m_Obstacles;
    float                   m_fBrakingWeight;

#ifdef _DEBUG
public:
    void                    DebugRender( sf::RenderWindow* _pRenderWindow ) const;
#endif // _DEBUG

};

/************************** Inline ************************/
inline void SteeringManager::SetComputeMethod( eComputeMethod _eMethod ) {
    m_eComputeMethod = _eMethod;
}

inline bool SteeringManager::IsOn( eBehaviors _eBehavior ) const {
    return (m_iActivatedBehaviors & _eBehavior) != 0;
}

inline void SteeringManager::On( eBehaviors _eBehavior ) {
    m_iActivatedBehaviors |= _eBehavior;
}

inline void SteeringManager::AllOff() {
    m_iActivatedBehaviors = 0;
}

inline void SteeringManager::Off( eBehaviors _eBehavior ) {
    m_iActivatedBehaviors ^= _eBehavior;
}

inline void SteeringManager::SetEntity( core::MovableEntity* _pEntity ) {
    m_pEntity = _pEntity;
}

inline core::MovableEntity* SteeringManager::GetEntity() const {
    return m_pEntity;
}

inline const sf::Vector2f& SteeringManager::GetSteeringResult() const {
    return m_SteeringResult;
}

    } // namespace ai
} // namespace smeg

#include <SmegTools/Debug/DebugNewOff.hpp>

#endif // STEERING_MANAGER_HPP
